# Copyright (c) 2025 Deep Robotics
# SPDX-License-Identifier: BSD 3-Clause

# Copyright (c) 2024-2025 Ziqi Fan
# SPDX-License-Identifier: Apache-2.0

# 导入isaaclab仿真相关工具，简化为sim_utils
import isaaclab.sim as sim_utils
# 从isaaclab.actuators模块导入直流电机和延迟PD执行器的配置类
from isaaclab.actuators import DCMotorCfg, DelayedPDActuatorCfg
# 导入关节配置类ArticulationCfg
from isaaclab.assets.articulation import ArticulationCfg

# 导入资产数据目录常量
from rl_training.assets import ISAACLAB_ASSETS_DATA_DIR

# 定义DeepRobotics Lite3机器人的关节配置
DEEPROBOTICS_LITE3_CFG = ArticulationCfg(
    # 机器人生成相关的URDF文件配置
    spawn=sim_utils.UrdfFileCfg(
        fix_base=False, # 是否固定基座
        merge_fixed_joints=True, # 是否合并固定关节
        replace_cylinders_with_capsules=True, # 是否用胶囊体替换圆柱体
        asset_path=f"{ISAACLAB_ASSETS_DATA_DIR}/Robots/deeprobotics/lite3_description/urdf/lite3.urdf", # 机器人URDF文件路径
        activate_contact_sensors=True, # 是否激活接触传感器
        # 刚体属性配置
        rigid_props=sim_utils.RigidBodyPropertiesCfg(
                disable_gravity=False, # 是否禁用重力
                retain_accelerations=False, # 是否保留加速度
                linear_damping=0.0, # 线性阻尼
                angular_damping=0.0, # 角阻尼
                max_linear_velocity=1000.0, # 最大线速度
                max_angular_velocity=1000.0, # 最大角速度
                max_depenetration_velocity=1.0, # 最大去穿透速度
        ),
        # 关节根属性配置
        # 如果想要仿真的更准确，可以增加solver_position_iteration_count和solver_velocity_iteration_count的值，
        # 数值越大，仿真计算越精确，但计算开销也会增加。
        articulation_props=sim_utils.ArticulationRootPropertiesCfg(
                enabled_self_collisions=False,     # 禁用自碰撞
                solver_position_iteration_count=4, # 位置迭代次数
                solver_velocity_iteration_count=1  # 速度迭代次数
        ),
        # 关节驱动配置
        joint_drive=sim_utils.UrdfConverterCfg.JointDriveCfg(
                gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg(stiffness=0, damping=0) # PD增益配置
        ),
    ),
    # 初始状态配置
    init_state=ArticulationCfg.InitialStateCfg(
        # 初始位置
        pos=(0.0, 0.0, 0.35),
        # 关节初始位置
        joint_pos={
                ".*HipX_joint": 0.0,  # 所有HipX关节初始为0
                ".*HipY_joint": -0.8, # 所有HipY关节初始为-0.8
                ".*Knee_joint": 1.6,  # 所有Knee关节初始为1.6
        }, 
        # 所有关节初始速度为0
        joint_vel={".*": 0.0},
    ),
    soft_joint_pos_limit_factor=0.99, # 软关节位置限制因子
    # 执行器配置
    actuators={
        # Hip关节使用延迟PD执行器
        "Hip": DelayedPDActuatorCfg(
                joint_names_expr=[".*_Hip[X,Y]_joint"], # 匹配所有Hip关节
                effort_limit=24.0,   # 最大输出力
                velocity_limit=26.2, # 最大速度
                stiffness=30.0, # 刚度
                damping=1.0,  # 阻尼
                friction=0.0, # 摩擦
                armature=0.0, # 转动惯量
                min_delay=0,  # 最小延迟
                max_delay=5,  # 最大延迟，机器人延迟通常在0~25ms之间；仿真物理步长是5ms，max_delay定义了有多少物理步长会被延迟
        ),
        "Knee": DelayedPDActuatorCfg(
            joint_names_expr=[".*_Knee_joint"],
            effort_limit=36.0,
            velocity_limit=17.3,
            stiffness=30.0,
            damping=1.0,
            friction=0.0,
            armature=0.0,
            min_delay=0,
            max_delay=5,
        ),
    },
)

# 定义DeepRobotics M20机器人的关节配置
DEEPROBOTICS_M20_CFG = ArticulationCfg(
    spawn=sim_utils.UrdfFileCfg(
        fix_base=False,
        merge_fixed_joints=True,
        replace_cylinders_with_capsules=True,
        asset_path=f"{ISAACLAB_ASSETS_DATA_DIR}/Robots/deeprobotics/m20_description/urdf/m20.urdf",
        activate_contact_sensors=True,
        rigid_props=sim_utils.RigidBodyPropertiesCfg(
            disable_gravity=False,
            retain_accelerations=False,
            linear_damping=0.0,
            angular_damping=0.0,
            max_linear_velocity=1000.0,
            max_angular_velocity=1000.0,
            max_depenetration_velocity=1.0,
        ),
        articulation_props=sim_utils.ArticulationRootPropertiesCfg(
            enabled_self_collisions=False, solver_position_iteration_count=4, solver_velocity_iteration_count=0
        ),
        joint_drive=sim_utils.UrdfConverterCfg.JointDriveCfg(
            gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg(stiffness=0, damping=0)
        ),
    ),
    init_state=ArticulationCfg.InitialStateCfg(
        pos=(0.0, 0.0, 0.52),
        joint_pos={
            ".*hipx_joint": 0.0,
            "f[l,r]_hipy_joint": -0.6,
            "h[l,r]_hipy_joint": 0.6,
            "f[l,r]_knee_joint": 1.0,
            "h[l,r]_knee_joint": -1.0,
            ".*wheel_joint": 0.0,
        },
        joint_vel={".*": 0.0},
    ),
    soft_joint_pos_limit_factor=0.9,
    actuators={
        "joint": DCMotorCfg(
            joint_names_expr=[".*hipx_joint", ".*hipy_joint", ".*knee_joint"],
            effort_limit=76.4,
            saturation_effort=76.4,
            velocity_limit=22.4,
            stiffness=80.0,
            damping=2.0,
            friction=0.0,
        ),
        "wheel": DCMotorCfg(
            joint_names_expr=[".*_wheel_joint"],
            effort_limit=21.6,
            saturation_effort=21.6,
            velocity_limit=79.3,
            stiffness=0.0,
            damping=0.6,
            friction=0.0,
        ),
    },
)

# 定义DeepRobotics Duck机器人的关节配置
DEEPROBOTICS_DUCK_CFG = ArticulationCfg(
    # 机器人生成相关的URDF文件配置
    spawn=sim_utils.UrdfFileCfg(
        fix_base=False, # 是否固定基座
        merge_fixed_joints=True, # 是否合并固定关节
        replace_cylinders_with_capsules=True, # 是否用胶囊体替换圆柱体
        asset_path=f"{ISAACLAB_ASSETS_DATA_DIR}/Robots/deeprobotics/duck_description/urdf/duck_10dofs.urdf", # 机器人URDF文件路径
        activate_contact_sensors=True, # 是否激活接触传感器
        # 刚体属性配置
        rigid_props=sim_utils.RigidBodyPropertiesCfg(
                disable_gravity=False, # 是否禁用重力
                retain_accelerations=False, # 是否保留加速度
                linear_damping=0.0, # 线性阻尼
                angular_damping=0.0, # 角阻尼
                max_linear_velocity=1000.0, # 最大线速度
                max_angular_velocity=1000.0, # 最大角速度
                max_depenetration_velocity=1.0, # 最大去穿透速度
        ),
        # 关节根属性配置
        # 如果想要仿真的更准确，可以增加solver_position_iteration_count和solver_velocity_iteration_count的值，
        # 数值越大，仿真计算越精确，但计算开销也会增加。
        articulation_props=sim_utils.ArticulationRootPropertiesCfg(
                enabled_self_collisions=False,     # 禁用自碰撞
                solver_position_iteration_count=4, # 位置迭代次数
                solver_velocity_iteration_count=1  # 速度迭代次数
        ),
        # 关节驱动配置
        joint_drive=sim_utils.UrdfConverterCfg.JointDriveCfg(
                gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg(stiffness=0, damping=0) # PD增益配置
        ),
    ),
    # 初始状态配置
    init_state=ArticulationCfg.InitialStateCfg(
        # 初始位置
        pos=(0.0, 0.0, 0.35),
        # 关节初始位置
        joint_pos={
                ".*hip_yaw_joint": 0.0,
                ".*hip_roll_joint": 0.0,
                ".*hip_pitch_joint": 0.7, 
                ".*knee_joint": -0.83,    
                ".*ankle_joint": 0.2,     
        }, 
        # 所有关节初始速度为0
        joint_vel={".*": 0.0},
    ),
    soft_joint_pos_limit_factor=0.99, # 软关节位置限制因子
    # 执行器配置
    actuators={
        # Hip关节使用延迟PD执行器
        "hip_yaw": DelayedPDActuatorCfg(
                joint_names_expr=[".*_hip_yaw_joint"],
                effort_limit=23.7,   # 最大输出力
                velocity_limit=30, # 最大速度
                stiffness=25.0, # 刚度
                damping=0.5,  # 阻尼
                friction=0.0, # 摩擦
                armature=0.0, # 转动惯量
                min_delay=0,  # 最小延迟
                max_delay=5,  # 最大延迟，机器人延迟通常在0~25ms之间；仿真物理步长是5ms，max_delay定义了有多少物理步长会被延迟
        ),
        "hip_roll": DelayedPDActuatorCfg(
                joint_names_expr=[".*_hip_roll_joint"],
                effort_limit=33.5,
                velocity_limit=21,
                stiffness=40.0,
                damping=1.0,
                friction=0.0,
                armature=0.0,
                min_delay=0,
                max_delay=5,
        ),
        "hip_pitch": DelayedPDActuatorCfg(
                joint_names_expr=[".*_hip_pitch_joint"],
                effort_limit=33.5,
                velocity_limit=21,
                stiffness=40.0,
                damping=1.0,
                friction=0.0,
                armature=0.0,
                min_delay=0,
                max_delay=5,
        ),
        "knee": DelayedPDActuatorCfg(
                joint_names_expr=[".*_knee_joint"],
                effort_limit=33.5,
                velocity_limit=21,
                stiffness=40.0,
                damping=1.0,
                friction=0.0,
                armature=0.0,
                min_delay=0,
                max_delay=5,
        ),
        "ankle": DelayedPDActuatorCfg(
            joint_names_expr=[".*_ankle_joint"],
            effort_limit=23.7,
            velocity_limit=30,
            stiffness=25.0,
            damping=0.5,
            friction=0.0,
            armature=0.0,
            min_delay=0,
            max_delay=5,
        ),
    },
)
